#include "uwbautofollow/uwbautofollow.h"

#include <std_msgs/Int32MultiArray.h>
#include <std_msgs/Float32MultiArray.h>

UwbAutoFollow::UwbAutoFollow(ros::NodeHandle &m_handle)
{

  ros::NodeHandle private_nh("~");

  private_nh.param<bool>("bdebug", bdebug, false);

  private_nh.param<bool>("avoid_danger_switch", avoid_danger_switch, true);
  private_nh.param<float>("dis_expect", dis_expect, 2.5);
  private_nh.param<float>("dis_range", dis_range, 0.5);

  private_nh.param<float>("stop_distance", stop_distance, 0.6);
  private_nh.param<float>("danger_distance", danger_distance, 2.0);
  private_nh.param<float>("side_danger_distance", side_danger_distance, 1.0);

  private_nh.param<float>("slow_down_distance", slow_down_distance, 1.5);
  private_nh.param<int>("follow_me_speed", follow_me_speed, 20);
  private_nh.param<int>("avoid_danger_slow_down_speed", avoid_danger_slow_down_speed, 10);
  private_nh.param<int>("avoid_danger_turn_angle", avoid_danger_turn_angle, 30);
  private_nh.param<int>("max_speed", MAX_SPEED, 30);

  pub_control = m_handle.advertise<geometry_msgs::Twist>("/cmd_vel", 1); // 发布节点

  pub_test_obstacles_raw = m_handle.advertise<std_msgs::Int32MultiArray>("/obstacle_grid", 10);               // 发布节点
  pub_test_obs_angle_inflation = m_handle.advertise<std_msgs::Float32MultiArray>("/obstacle_angle_grid", 10); // 发布节点

  // sub_ultrasonic = m_handle.subscribe("/psd_ultrasonic", 1, &UwbAutoFollow::callback_ultrasonic, this);   // 订阅节点
  sub_uwb = m_handle.subscribe("/nlink_linktrack_aoa_nodeframe0", 1, &UwbAutoFollow::callback_uwb, this); // 订阅节点
  sub_laserscan = m_handle.subscribe("/scan", 1, &UwbAutoFollow::callback_laserscan, this);               // 订阅节点

  PID_Init();

  ROS_INFO_STREAM("UwbAutoFollow Init OK!"); // Prompt message //提示信息
}

UwbAutoFollow::~UwbAutoFollow() // 析构函数，在停止对象调用时调用
{
}

void UwbAutoFollow::Publish_obstacle_grid(void)
{
  std_msgs::Int32MultiArray msg;

  std_msgs::MultiArrayDimension d1, d2;
  d1.label = "height";
  d1.stride = GRID_NUM * GRID_NUM;
  d1.size = GRID_NUM;
  d2.label = "width";
  d2.stride = GRID_NUM;
  d2.size = GRID_NUM;

  std_msgs::MultiArrayLayout layout;
  layout.data_offset = 0;
  layout.dim = std::vector<std_msgs::MultiArrayDimension>({d1, d2});
  msg.layout = layout;
  // 直接定义一个一维的vector，其首地址为数组的起始地址，末尾地址为最后的地址。
  msg.data = std::vector<int32_t>(m_obs_grid.obstacle_raw[0], m_obs_grid.obstacle_raw[0] + GRID_NUM * GRID_NUM);

  pub_test_obstacles_raw.publish(msg);
}

void UwbAutoFollow::Publish_obstacle_angle(void)
{
  std_msgs::Float32MultiArray msg;

  // 直接定义一个一维的vector，其首地址为数组的起始地址，末尾地址为最后的地址。
  msg.data = std::vector<float>(m_obs_grid.obs_angle_inflation, m_obs_grid.obs_angle_inflation + GRID_ANGLE_LEN);

  pub_test_obs_angle_inflation.publish(msg);
}

void UwbAutoFollow::callback_laserscan(const sensor_msgs::LaserScan::ConstPtr &msg)
{
  // ROS_INFO("callback_laserscan.");

  lindar_rx_timeout = 1000;

  m_obs_grid.process_data(msg);

  Publish_obstacle_grid();
  Publish_obstacle_angle();
}

void UwbAutoFollow::callback_uwb(const nlink_parser::LinktrackAoaNodeframe0::ConstPtr &message)
{

  if (bdebug)
  {
    ROS_INFO("msg.role: %d", message->role);
    ROS_INFO("msg.id: %d", message->id);
    ROS_INFO("msg.nodes.size(): %ld", message->nodes.size());
    if (message->nodes.size() > 0)
    {
      ROS_INFO("msg.nodes[0].dis: %.2f", message->nodes[0].dis);
      ROS_INFO("msg.nodes[0].angle: %.2f", message->nodes[0].angle);
      ROS_INFO("msg.nodes[0].fp_rssi: %.2f", message->nodes[0].fp_rssi);
      ROS_INFO("msg.nodes[0].rx_rssi: %.2f", message->nodes[0].rx_rssi);
    }
  }
  // dis * 1000 int24 3 Distance from the tag to the corresponding anchor, unit: m
  // angle*100 int16 2 Range: [-180,180], unit: deg

  if (message->nodes.size() > 0) // 如果nodes[0]数据有效且解码正确
  {
    uwb_rx_timeout = 1000;
    angle_measure_uwb = message->nodes[0].angle; // 取AOA测量角度值 left: positive, right: negtive
    dis_measure_uwb = message->nodes[0].dis;     // 取AOA测量距离值
    AOA_Application();
  }
  else
  {
    // 如果可用节点数=0，小车保持不动
    Move_Control(y1_output + 0, z1_output + 0);
  }
}
void UwbAutoFollow::callback_ultrasonic(const xfultrasonicdriver::ultrasonic_msg::ConstPtr &msg)
{
  if (bdebug)
  {
    ROS_INFO("psd  I: %3d %3d %3d %3d %3d %3d", msg->psd01, msg->psd02, msg->psd03, msg->psd04, msg->psd05, msg->psd06);
    // ROS_INFO("psd II: %3d %3d %3d %3d %3d %3d", msg->psd07, msg->psd08, msg->psd09, msg->psd10, msg->psd11, msg->psd12);
  }
  // m_psd.setDistanceFront(msg->psd01, msg->psd02, msg->psd03, msg->psd04, msg->psd05, msg->psd06);
  //  m_psd.setDistanceBack(msg->psd07, msg->psd08, msg->psd09, msg->psd10, msg->psd11, msg->psd12);

  // psd_filter();
}

void UwbAutoFollow::follow_me()
{
}

void UwbAutoFollow::run()
{

  ros::Rate rate(100); // 100Hz
  int count = 0;
  while (ros::ok())
  {
    ros::spinOnce();

    if (left_obstacle_timeout >= 10)
    {
      left_obstacle_timeout -= 10;
    }
    else
    {
      left_obstacle_timeout = 0;
    }
    if (right_obstacle_timeout >= 10)
    {
      right_obstacle_timeout -= 10;
    }
    else
    {
      right_obstacle_timeout = 0;
    }

    if (uwb_rx_timeout >= 10)
    {
      uwb_rx_timeout -= 10;
    }
    else
    {
      uwb_rx_timeout = 0;
      Move_Control(y1_output + 0, z1_output + 0);
      ROS_ERROR("uwb_rx_timeout");
    }

    if (lindar_rx_timeout >= 10)
    {
      lindar_rx_timeout -= 10;

      // for test
      // uwb_rx_timeout = 1000;
      // angle_measure_uwb = 30; // 取AOA测量角度值 left: positive, right: negtive
      // dis_measure_uwb = 2.5;  // 取AOA测量距离值
      // AOA_Application();
    }
    else
    {
      lindar_rx_timeout = 0;
      Move_Control(y1_output + 0, z1_output + 0);
      ROS_ERROR("lindar_rx_timeout");
    }

    rate.sleep();
  }
}

/************************************************
函数名称 ： AOA_Application
函数功能 ： AOA跟随应用函数
参    数 ： 无
返 回 值 ： 无
*************************************************/
void UwbAutoFollow::AOA_Application()
{

  if (lindar_rx_timeout <= 0)
  {
    return;
  }

  debug_str = "";

  /*跟随模式处理代码 开始*/
  avoid_danger_status = 0;

  y1_increase = follow_me_speed; // 初始跟随速度

  z1_increase = (int)PID_Loc(angle_expect, angle_measure_uwb, &AOA_PID); // PID算出输出量

  z1_increase = angle_measure_uwb;

  if (fabsf(angle_measure_uwb) < angle_target_range) // 判断角度测量值是否达到了目标环内
  {
    deadzone_angle_range_flag = 1; // 如果达到目标环，进入死区环判断并置位标志位
  }
  if (deadzone_angle_range_flag == 1) // 如果死区环标志位为1
  {
    if (fabsf(angle_measure_uwb) < angle_dead_zone_range) // 如果角度波动范围小于死区环范围
    {
      z1_increase = 0; // 输出改为0
    }
    else
    {
      deadzone_angle_range_flag = 0; // 如果角度波动范围大于死区环范围，标志位清零
    }
  }

  if (avoid_danger_switch) // 如果开启了避障模式
  {
    avoid_danger_status = 1; // 如果符合避障的条件，避障控制标志位置1
    if (deadzone_obs_stop_flag > 0)
    {
      if (!m_obs_grid.detect_obstacle(0.2 + 0.2 + 0.6, stop_distance + 0.2)) // Diff_wheelSpacing = 0.6
      {
        debug_str += "1";
        deadzone_obs_stop_flag = 0;
      }
      else
      {
        debug_str += "2";

        y1_increase = 0;
        z1_increase = 0;
      }
    }
    else
    {
      if (m_obs_grid.detect_obstacle(0.1 + 0.1 + 0.6, stop_distance)) // Diff_wheelSpacing = 0.6
      {
        debug_str += "3";

        deadzone_obs_stop_flag = 1;
        y1_increase = 0;
        z1_increase = 0;
      }
    }

    if (deadzone_obs_stop_flag == 0 && deadzone_distance_range_flag == 0)
    {
      float tmpAngle = m_obs_grid.find_anlge_of_way(z1_increase, dis_measure_uwb, danger_distance);

      ROS_INFO("find_anlge_of_way: %.2f", tmpAngle); // Prompt message //提示信息

      if (tmpAngle != -180)
      {
        debug_str += "4";
        z1_increase = tmpAngle;
      }
      else
      {
        debug_str += "5";
        y1_increase = 0;
        z1_increase = 0;
      }
    }
  }

  if (avoid_danger_status == 0)
  {
    ;
  }

  if (dis_measure_uwb < dis_expect) // 判断角度测量值是否达到了目标环内
  {
    deadzone_distance_range_flag = 1; // 如果达到目标环，进入死区环判断并置位标志位
  }

  if (deadzone_distance_range_flag == 1) // 如果死区环标志位为1
  {
    if (dis_measure_uwb < dis_expect + dis_range) // 如果角度波动范围小于死区环范围
    {
      debug_str += "6";
      y1_increase = 0; // 输出改为0
    }
    else
    {
      deadzone_distance_range_flag = 0; // 如果角度波动范围大于死区环范围，标志位清零
    }
  }

  /*跟随模式处理代码 结束*/

  debug_str += "(" + std::to_string(angle_measure_uwb) + "," + std::to_string(z1_increase) + "," + std::to_string(y1_increase) + ")";

  // ROS_INFO("debug_str: %s", debug_str.c_str()); // Prompt message //提示信息

  Move_Control(y1_output + y1_increase, z1_output + z1_increase);
}

// #define M_PI 3.14159265358979323846
void UwbAutoFollow::Move_Control(int16_t speed, int16_t turn_angle) // 运动控制
{

  geometry_msgs::Twist msg;
  msg.linear.x = 0;
  msg.linear.y = 0;
  msg.linear.z = 0;

  msg.angular.x = 0;
  msg.angular.y = 0;
  msg.angular.z = 0;

  if (turn_angle < -90)
    turn_angle = -90;
  else if (turn_angle > 90)
    turn_angle = 90;

  if (speed < -MAX_SPEED)
    speed = -MAX_SPEED;
  else if (speed > MAX_SPEED)
    speed = MAX_SPEED;

  msg.linear.x = speed / 100.0f;               // cm/s to m/s
  msg.angular.z = turn_angle * (M_PI / 180.0); // degree to rad

  // for test
  //  msg.linear.x = 0.2;
  //  msg.angular.z = 30 * (M_PI / 180.0);

  pub_control.publish(msg);
}

void UwbAutoFollow::debug_out(int debug_id)
{
  ROS_INFO("debug_id: %d", debug_id); // Prompt message //提示信息
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "uwbautofollow");
  ros::NodeHandle nh;
  UwbAutoFollow Renode(nh);
  Renode.run();
  return 0;
}
